#include "CRobotSturing.h"
//#include "math.h"
#include <cmath>
using namespace std;

CRobotSturing::CRobotSturing()
{

}

CRobotSturing::~CRobotSturing()
{

} 

void CRobotSturing::gaNaar(int x, int y)
{
	m_nieuwX = x;
	m_nieuwY = y;
	stuur();
}

Robot CRobotSturing::getRobot()
{
	return m_robot;
}

void CRobotSturing::setRobot(Robot &r)
{
	m_robot = r;
}

short CRobotSturing::berekenDraaiHoek()
{
	// draaien in graden
	if ( (m_nieuwX > m_huidigX) && (m_robot.getHoek() == (int)0) )
		return 0;
	else if ( (m_nieuwX < m_huidigX) && (m_robot.getHoek() == (int)180) )
		return 0;
	else
		return 180;
}

void CRobotSturing::stuur()
{
	//bool finish = false;
	printf("-stuur-----\n");
	m_motor.Rotate(berekenDraaiHoek(),true);
	
	m_motor.Drive(100);
	//while (!finish)
	{
		if (abs((double)m_nieuwX - m_robot.getPositieX()) < 1)
		{
			//finish = true;
			m_motor.Drive(0);
		}
	}
}
